import sys
import signal
from PyQt5.QtWidgets import QApplication, QWidget, QPushButton, QLabel, QTextEdit, QVBoxLayout, QHBoxLayout, QDesktopWidget
from PyQt5.QtGui import QFont, QPixmap, QImage
from PyQt5.QtCore import Qt, QThread, pyqtSignal, QSize, QTimer

import socket
import struct
import numpy as np
import cv2
import socket

class VideoThread(QThread):
    update_frame = pyqtSignal(QImage)

    def __init__(self, addr_port=("192.168.43.198", 1234)):
        super().__init__()
        self.addr_port = addr_port
        self.sk = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.running = True

    def run(self):
        print("视频服务器正在监听...")
        # cv2.namedWindow("Remote Video", cv2.WINDOW_NORMAL)
        self.sk.bind(self.addr_port) 

        while self.running:
            try:
                data, addr = self.sk.recvfrom(88888)
                if not data:
                    print("Connection closed")
                    continue

                if len(data) < struct.calcsize("Q"):
                    print("Data is incomplete")
                    continue

                packed_msg_size = data[:struct.calcsize("Q")]
                msg_size = struct.unpack("Q", packed_msg_size)[0]
                frame_data = data[struct.calcsize("Q"):]

                if len(frame_data) < msg_size:
                    continue

                frame = np.frombuffer(frame_data, dtype=np.uint8)
                frame = cv2.imdecode(frame, cv2.IMREAD_COLOR)
                # cv2.imshow("Remote Video", frame)
                h, w, ch = frame.shape
                bytes_per_line = ch * w
                # qt_frame = QImage(frame.data, w, h, bytes_per_line, QImage.Format_RGB888)
                qt_frame = QImage(frame.data, w, h, bytes_per_line, QImage.Format_BGR888)
                self.update_frame.emit(qt_frame)

                if cv2.waitKey(1) & 0xFF == 27:
                    break

            except Exception as e:
                    print(e)
    def stop(self):
        self.running = False
        self.sk.close()
        self.wait()

class GPSThread(QThread):
    update_gps = pyqtSignal(str)

    def __init__(self, addr_port=("192.168.43.198", 1005)):
        super().__init__()
        self.addr_port = addr_port
        self.sk = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.running = True
        
    def run(self):
        self.sk.bind(self.addr_port)

        while self.running:
            try:
                data, addr = self.sk.recvfrom(1024)
                
                if not data:
                    print("Connection closed")
                    continue
                
                self.update_gps.emit(data.decode())

            except Exception as e:
                print(e)

    def stop(self):
        self.running = False
def initialize_command_socket():
    global cmd_sk
    ip_port = ("192.168.43.186", 9999)
    #ip_port = ("192.168.43.134", 9999)
    cmd_sk = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    cmd_sk.connect(ip_port)

initialize_command_socket()


class DoubleBufferedWindow(QWidget):
    def __init__(self):
        super().__init__()
        self.initUI()

    def initUI(self):
        self.setGeometry(100, 100, 640, 360)
        self.setWindowTitle('机器狗控制面板')
        self.center_window()
        self.setMinimumSize((QSize(640, 800)))
        self.setMaximumSize((QSize(640, 800)))
        

        # 视频显示区域
        self.video_label = QLabel(self)
        self.video_label.setSizePolicy(self.sizePolicy())
        self.video_label.setAlignment(Qt.AlignCenter)

        # 创建显示按键触发反馈的标签
        self.feedback_label = QLabel('', self)
        self.feedback_label.setFont(QFont('Arial', 18))
        self.feedback_label.setAlignment(Qt.AlignCenter)

        # GPS显示区域
        self.gps_display = QTextEdit(self)
        self.gps_display.setReadOnly(True)
        self.gps_display.setFixedSize(200, 100)

        # 创建主布局
        vbox = QVBoxLayout()
        vbox.addWidget(self.video_label)
        vbox.addWidget(self.feedback_label)
        vbox.addWidget(QLabel("GPS Location:", self))
        vbox.addWidget(self.gps_display)

        # 定义按键与其功能的对应关系
        self.key_functions = {
            'q': '增加左偏航角度',
            'w': '增加前进速度 / 减少俯仰角度',
            'e': '增加右偏航角度',
            'u': '退出身体运动指令模式并返回休息模式',
            'a': '增加左移速度 / 增加左滚动角度',
            's': '增加后退速度 / 增加俯仰角度',
            'd': '增加右移速度 / 增加右滚动角度',
            'f': '在行走模式下，将所有速率指令清零',
            'stand': '让机器狗站立起来',
            'walk': '启动行走模式并通过键盘控制运动',
            'idle': '让机器狗趴下',
            'angle_cmd': '转动机器狗身体',
            'find_people': '寻人模式'
        }

        # 第一行按钮 (stand, walk, idle)
        # hbox1 = QHBoxLayout()
        # for command in ['stand', 'walk', 'idle']:
        #     button = QPushButton(command.upper(), self)
        #     button.setStyleSheet(self.button_style())
        #     button.clicked.connect(lambda checked, cmd=command: self.on_command_click(cmd))
        #     hbox1.addWidget(button)

        hbox1 = QHBoxLayout()
        for command in ['stand', 'walk', 'idle', 'angle_cmd', 'find_people']:
            button = QPushButton(command.upper(), self)
            button.setStyleSheet(self.button_style())
            button.clicked.connect(lambda checked, cmd=command: self.on_command_click(cmd))
            hbox1.addWidget(button)

        # 第二行按钮 (q, w, e, u)
        hbox2 = QHBoxLayout()
        buttons_row2 = ['q', 'w', 'e', 'u']
        for key in buttons_row2:
            button = QPushButton(key.upper(), self)
            button.setShortcut(key)
            button.setStyleSheet(self.button_style())
            button.clicked.connect(lambda checked, k=key: self.on_key_click(k))
            hbox2.addWidget(button)

        # 第三行按钮 (a, s, d, f)
        hbox3 = QHBoxLayout()
        buttons_row3 = ['a', 's', 'd', 'f']
        for key in buttons_row3:
            button = QPushButton(key.upper(), self)
            button.setShortcut(key)
            button.setStyleSheet(self.button_style())
            button.clicked.connect(lambda checked, k=key: self.on_key_click(k))
            hbox3.addWidget(button)

        vbox.addLayout(hbox1)
        vbox.addLayout(hbox2)
        vbox.addLayout(hbox3)
 
        self.setLayout(vbox)
        self.showMaximized()

        # 启动视频流线程（提高初始化速度）
        self.video_thread = VideoThread(addr_port=("192.168.43.198", 1234))
        self.video_thread.update_frame.connect(self.update_video_frame)
        self.video_thread.start()
        
        # 启动GPS线程
        self.gps_thread = GPSThread(addr_port=("192.168.43.198", 1005))
        self.gps_thread.update_gps.connect(self.update_gps_display)
        self.gps_thread.start()

        # 设置Ctrl+C退出功能
        signal.signal(signal.SIGINT, self.signal_handler)
        
        # 使用定时器定期检查信号处理
        self.timer = QTimer()
        self.timer.timeout.connect(self.check_for_exit)
        self.timer.start(500)  # 每500毫秒检查一次

    def button_style(self):
        return """
            QPushButton {
                background-color: #4CAF50;
                border: none;
                color: white;
                padding: 15px 30px;
                text-align: center;
                text-decoration: none;
                font-size: 20px;
                margin: 6px 4px;
                border-radius: 10px;
            }
            QPushButton:pressed {
                background-color: #45a049;
            }
        """

    def update_video_frame(self, frame):
        self.video_label.setPixmap(QPixmap.fromImage(frame))

    def update_gps_display(self, gps_data):
        self.gps_display.append(gps_data)

    def resizeEvent(self, event):
        self.video_label.resize(event.size())

    def center_window(self):
        qr = self.frameGeometry()
        cp = QDesktopWidget().availableGeometry().center()
        qr.moveCenter(cp)
        self.move(qr.topLeft())

    def on_command_click(self, command):
        function = self.key_functions.get(command, "未知功能")
        feedback_text = f"{command.upper()} - {function}"
        self.feedback_label.setText(feedback_text)
        print(feedback_text)
        cmd_sk.sendall(command.encode())
        print(f"发送命令到nano: {command}")

    def on_key_click(self, key):
        function = self.key_functions.get(key, "未知功能")
        feedback_text = f" {key.upper()}  - {function}"
        self.feedback_label.setText(feedback_text)
        print(feedback_text)
        cmd_sk.sendall(key.encode())
        print(f"发送命令到nano: {key}")

    # def send_to_nano(self, command):
    #     # 实现将命令发送到nano的逻辑
    #     cmd_sk.sendall(key.encode())
    #     print(f"发送命令到nano: {command}")
    def keyPressEvent(self, event):
        key_text = event.text().lower()
        if key_text in self.key_functions:
            self.on_key_click(key_text)
        elif key_text in ['stand', 'idle', 'walk', 'angle_cmd', 'find_people']:
            self.on_command_click(key_text)
        elif event.key() == Qt.Key_C and event.modifiers() == Qt.ControlModifier:
            print("CTRL-C 被触发，程序退出")
            self.close()

    def closeEvent(self, event):
        self.video_thread.stop()
        self.gps_thread.stop()
        self.timer.stop()
        event.accept()

    def signal_handler(self, sig, frame):
        print("CTRL-C 被触发，程序退出")
        self.close()

    def check_for_exit(self):
        if not self.video_thread.isRunning():
            QApplication.quit()

if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = DoubleBufferedWindow()
    sys.exit(app.exec_())
